#ifndef CUDA_POISSON_RRT_H
#define CUDA_POISSON_RRT_H

#include "GPUPoissonRRT.h"
#include "cuda_rrt_types.h"

class GMesh;

namespace poisson
{

class CUDAPoissonRRT : public GPUPoissonRRT
{
public:
	CUDAPoissonRRT(const ompl::base::SpaceInformationPtr &si, const ompl::app::GeometrySpecification* geometrySpecification);
	virtual ~CUDAPoissonRRT();

	virtual ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc);
	virtual void clear(void);
	virtual void setup(void);

protected:
	virtual void freeMemory(void);

	void reset();
	void initializeMemory();
	void uploadData(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax, 
		const GMesh* h_robot, const GMesh* h_obstacle, 
		const std::vector<SerializedStates>& vSerializedStates, const std::vector<SerializedStates>& vTemplateStates);

	// primitive methods
	void generateRandomStates(SerializedStates* outStates);
	void computeNearestNodes(RrtTree& tree, SerializedStates* inStates, float* outDistances, int* outNodeIndices);
	void computeNeighborSample(RrtTree& tree, int* inNodeIndices, SerializedStates* inDestStates, int* outSampleIndices);
	void checkCollision(RrtTree& currentTree, int* inNodeIndices, int* inDestStateIndices,
		int* outResults, SerializedStates* outColFreeStates);
	void removeDuplicatedPairs(int* inNodeIndices, int* inDestStateIndices, int* inResults);
	void addNewNodes(RrtTree& currentTree, RrtTree& otherTree, int* inNodeIndices, int* inDestStateIndices,
		int* inResults, SerializedStates* inColFreeStates);
	void generateDestStatesForOtherTree(int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates, SerializedStates* outStates);
	bool isSolutionFound(std::vector<SerializedStates>& solution);

	bool resetRequired_;

	float stateValidityCheckingResolutionR3_;
	float stateValidityCheckingResolutionSO3_;

	int numPrecomputedPDSamples_;
	int numSamples_;

	// pointers of device memories
	PoissonDiskSampleData d_sampleData_;
	PoissonDiskSampleData d_templateData_;
	RrtTree d_startTree_;
	RrtTree d_goalTree_;

	GMesh* d_robot_;
	GMesh* d_obstacle_;
};

}

#endif